/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-27     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"


/*-------------------------------------------------------------------*
 *      Z
 *      ^
 *      |__> Y
 *      \
 *       v
 *       X
 *-------------------------------------------------------------------*/
#define  Local_G     980.0f  //cm/s2
#define  ACCE_SCALE  16      //+-8g
#define  GYRO_SCALE  4000    //+-2000dsp

#define  ACCE_ADC_covert  (65536.0f/ACCE_SCALE)
#define  GYRO_ADC_covert  (65536.0f/GYRO_SCALE)

#define Calibration_Acce_Cycle         500.0f
#define Calibration_Gyro_Cycle         500.0f

IMU_HandleTypedef IMU_Handle;
static imuData_t imu;
static Axis3i16 gyro_ripe_adc, acce_ripe_adc;
static Axis3f   acce_ripe_dat, gyro_ripe_dat;
static Calibration_t gyroCalibration;
static Calibration_t acceCalibration;

static char mb_pool[4];
static struct rt_mailbox mb_sensor;

static void imu_ClibInit(void);

/* imu_Task */
extern rt_thread_t AHRS_thread;
void sensor_imuTask(void* parameter)
{
    rt_err_t result;
    /* 初始化一个mailbox */
    result = rt_mb_init(&mb_sensor, "mb_sensor", &mb_pool[0], 1, RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        rt_kprintf("failed to create a mb_sensor!\r\n");
    }

    imu_ClibInit();
    rt_thread_startup(AHRS_thread);
    while(1)
    {
        /* 原始数据获取 */
        IMU_Handle.getRawData();
        IMU_Handle.gyroUpdate(&imu.gyro);
        IMU_Handle.acceUpdate(&imu.acce);

        if(gyroCalibration.calibration_State == Calibration_OK && acceCalibration.calibration_State == Calibration_OK)
        {
            RC_Handle.Flyflag.Ready = 1;
            rt_mb_send(&mb_sensor, (rt_ubase_t)(&imu)); //通知姿态解算
        }
        rt_thread_mdelay(1);
    }
}


static uint8_t gyro_data_update(Axis3f *gyro)
{
    if(gyroCalibration.calibration_State != Calibration_OK)//calibration
    {
        IMU_Handle.gyroCali(IMU_Handle.rawGyro_adc);
    }
    if(gyroCalibration.calibration_State == Calibration_OK)
    {
        /* zero bias correction */
        gyro_ripe_adc.x = IMU_Handle.rawGyro_adc->x - gyroCalibration.zero.x;
        gyro_ripe_adc.y = IMU_Handle.rawGyro_adc->y - gyroCalibration.zero.y;
        gyro_ripe_adc.z = IMU_Handle.rawGyro_adc->z - gyroCalibration.zero.z;
        /* unit conversion */
        gyro_ripe_dat.x = gyro_ripe_adc.x / GYRO_ADC_covert;
        gyro_ripe_dat.y = gyro_ripe_adc.y / GYRO_ADC_covert;
        gyro_ripe_dat.z = gyro_ripe_adc.z / GYRO_ADC_covert;
        /* direction correction */
        gyro->x =  gyro_ripe_dat.x;
        gyro->y =  gyro_ripe_dat.y;
        gyro->z =  gyro_ripe_dat.z;// - 0.0017f;//飘移
    }
    return gyroCalibration.calibration_State;
}
static uint8_t acce_data_update(Axis3f *acce)
{
    if(acceCalibration.calibration_State != Calibration_OK) //calibration,save
    {
        IMU_Handle.acceCali(IMU_Handle.rawAcce_adc);

        if(acceCalibration.calibration_State == Calibration_OK)
        {
            Parame_Clib.acce.x = acceCalibration.zero.x;
            Parame_Clib.acce.y = acceCalibration.zero.y;
            Parame_Clib.acce.z = acceCalibration.zero.z;
            Parame_Clib.acceState = acceCalibration.calibration_State;
            Parame_FLAG.Clib_PSflag = 1;
        }
    }
    if(acceCalibration.calibration_State == Calibration_OK)
    {
        /* zero bias correction */
        acce_ripe_adc.x = IMU_Handle.rawAcce_adc->x - acceCalibration.zero.x;
        acce_ripe_adc.y = IMU_Handle.rawAcce_adc->y - acceCalibration.zero.y;
        acce_ripe_adc.z = IMU_Handle.rawAcce_adc->z - acceCalibration.zero.z;
        /* unit conversion */
        acce_ripe_dat.x = acce_ripe_adc.x / ACCE_ADC_covert * Local_G;
        acce_ripe_dat.y = acce_ripe_adc.y / ACCE_ADC_covert * Local_G;
        acce_ripe_dat.z = acce_ripe_adc.z / ACCE_ADC_covert * Local_G;
//        /* direction correction */
//        acce->x = -acce_ripe_dat.x;
//        acce->y = -acce_ripe_dat.y;
//        acce->z = -acce_ripe_dat.z;

        static Axis3f acce_lpf;
        acce_lpf.x =  acce_ripe_dat.x;
        acce_lpf.y =  acce_ripe_dat.y;
        acce_lpf.z =  acce_ripe_dat.z;

        for (uint8_t i = 0; i < 3; i++)
        {
            acce->axis[i] += 0.5*(acce_lpf.axis[i] - acce->axis[i]);
        }
    }
    return acceCalibration.calibration_State;
}

//static void tem_data_updata(float *tem)
//{
//
//}

static void variance_cal(variance_t *dev, int16_t x)
{
    dev->n++;
    if(dev->n == 1)
    {
        dev->E_n = dev->E_n_1 = x;
        dev->D_n = dev->D_n_1 = 0.0f;
    }
    else
    {
        dev->E_n = dev->E_n_1 + (x-dev->E_n_1)/(dev->n);//need optimize n or n+1
        dev->D_n = (dev->n-1)*dev->D_n_1/dev->n + (dev->n-1)*(dev->E_n - dev->E_n_1)*(dev->E_n - dev->E_n_1);
        dev->E_n_1 = dev->E_n;
        dev->D_n_1 = dev->D_n;
    }
}
static void cali_parameter_reset(Calibration_t *calibration)
{
    calibration->cycleCount = Calibration_Acce_Cycle;
    calibration->sum.x = 0;
    calibration->sum.y = 0;
    calibration->sum.z = 0;
    calibration->zero.x =0;
    for(uint8_t i = 0; i < 3; i++)
    {
        calibration->variance[i].E_n     = 0;
        calibration->variance[i].E_n_1   = 0;
        calibration->variance[i].D_n     = 0;
        calibration->variance[i].D_n_1   = 0;
        calibration->variance[i].n       = 0;
    }
}
static uint8_t gyro_calibration(Axis3i16 *gyro_raw_adc)
{
    switch(gyroCalibration.calibration_State)
    {
        case Calibration_OK: return 1;
        case Calibration_none:
        {
            gyroCalibration.calibration_State = Calibration_ing;
            cali_parameter_reset(&gyroCalibration);
        }break;
        case Calibration_ing:
        {
            for(uint8_t i = 0; i < 3; i++)
            {
                gyroCalibration.sum.axis[i] += gyro_raw_adc->axis[i];
                variance_cal(&gyroCalibration.variance[i], gyro_raw_adc->axis[i]);
            }
            if(gyroCalibration.cycleCount == 1)
            {
                for(uint8_t i = 0; i < 3; i++)
                {
                    if ((gyroCalibration.variance[i].D_n > 10)||(gyroCalibration.variance[i].D_n == 0))/////!
                    {
                        gyroCalibration.calibration_State = Calibration_failed;
                        goto Cali_failed;
                    }
                }
                /* calibration complete */
                gyroCalibration.zero.x = (gyroCalibration.sum.x) / Calibration_Gyro_Cycle;
                gyroCalibration.zero.y = (gyroCalibration.sum.y) / Calibration_Gyro_Cycle;
                gyroCalibration.zero.z = (gyroCalibration.sum.z) / Calibration_Gyro_Cycle;
                gyroCalibration.calibration_State = Calibration_OK;
                rt_kprintf("\r\nGyro_Calibration_OK\r\n");

                for(uint8_t i = 0; i < 3; i++)rt_kprintf("x_y_z variance:%f\r\n", gyroCalibration.variance[i].D_n);
//                    printf("Acce_Calibration_OK---variance:%f\r\n", gyroCalibration.variance[i].E_n);
//                    printf("Acce_Calibration_OK---variance:%d\r\n", gyroCalibration.variance[i].n);

                return RT_EOK;
            }
            gyroCalibration.cycleCount--;
        }break;
        case Calibration_failed:
        {   Cali_failed:
            rt_kprintf("Gyro_Calibration_FALSE\r\n");
            for(uint8_t i = 0; i < 3; i++)rt_kprintf("x_y_z variance:%f\r\n", gyroCalibration.variance[i].D_n);
            cali_parameter_reset(&gyroCalibration);
            gyroCalibration.calibration_State = Calibration_ing;
        }break;
        default:break;
    }
    return gyroCalibration.calibration_State;
}
static uint8_t acce_calibration(Axis3i16 *acce_raw_adc)
{
    switch(acceCalibration.calibration_State)
    {
        case Calibration_OK: return 1;
        case Calibration_none:
        {
            acceCalibration.calibration_State = Calibration_ing;
            cali_parameter_reset(&acceCalibration);
        }break;
        case Calibration_ing:
        {
            for(uint8_t i = 0; i < 3; i++)
            {
                acceCalibration.sum.axis[i] += acce_raw_adc->axis[i];
                variance_cal(&acceCalibration.variance[i], acce_raw_adc->axis[i]);
            }
            if(acceCalibration.cycleCount == 1)
            {
                for(uint8_t i = 0; i < 3; i++)
                {
                    if ((acceCalibration.variance[i].D_n > 800)||(acceCalibration.variance[i].D_n == 0))/////!
                    {
                        acceCalibration.calibration_State = Calibration_failed;
                        goto Cali_failed;
                    }
                }
                /* calibration complete */
                acceCalibration.zero.x = (acceCalibration.sum.x) / Calibration_Acce_Cycle;
                acceCalibration.zero.y = (acceCalibration.sum.y) / Calibration_Acce_Cycle;
                acceCalibration.zero.z = (acceCalibration.sum.z) / Calibration_Acce_Cycle + 4096;       //-4096 or +4096
                acceCalibration.calibration_State = Calibration_OK;

                rt_kprintf("\r\nAcce_Calibration_OK\r\n");
                //for(uint8_t i = 0; i < 3; i++)printf("x_y_z variance:%f\r\n", acceCalibration.variance[i].D_n);
                for(uint8_t i = 0; i < 3; i++)rt_kprintf("x_y_z variance:%f\r\n", acceCalibration.zero.axis[i]);
//                    printf("Acce_Calibration_OK---variance:%f\r\n", acceCalibration.variance[i].E_n);
//                    printf("Acce_Calibration_OK---variance:%d\r\n", acceCalibration.variance[i].n);

                return RT_EOK;
            }
            acceCalibration.cycleCount--;
        }break;
        case Calibration_failed:
        {   Cali_failed:
            rt_kprintf("Acce_Calibration_FALSE\r\n");
            for(uint8_t i = 0; i < 3; i++)rt_kprintf("x_y_z variance:%f\r\n", acceCalibration.variance[i].D_n);
            cali_parameter_reset(&acceCalibration);
            acceCalibration.calibration_State = Calibration_ing;
        }break;
        default:break;
    }
    return acceCalibration.calibration_State;
}

static void imu_ClibInit(void)
{
    acceCalibration.zero.x = Parame_Clib.acce.x;
    acceCalibration.zero.y = Parame_Clib.acce.y;
    acceCalibration.zero.z = Parame_Clib.acce.z;
    acceCalibration.calibration_State = Parame_Clib.acceState;
}

static int imu_Init(void)
{
    IMU_Handle.acceCali     = acce_calibration;
    IMU_Handle.gyroCali     = gyro_calibration;
    IMU_Handle.acceUpdate   = acce_data_update;
    IMU_Handle.gyroUpdate   = gyro_data_update;
    IMU_Handle.ripeAcce_adc = &acce_ripe_adc;
    IMU_Handle.ripeGyro_adc = &gyro_ripe_adc;
    IMU_Handle.imu_data     = &imu;
    IMU_Handle.CaliAcce     = &acceCalibration;
    IMU_Handle.CaliGyro     = &gyroCalibration;
    IMU_Handle.mb           = &mb_sensor;

    return 0;
}
INIT_PREV_EXPORT(imu_Init);

